/******************************************************************************
 *	Author: 	Williams, Jacob (williamsj@msoe.edu)
 *	Class:		CE-3910
 *	Instructor:	Dr. Barnekow
 *****************************************************************************/
#include "camera.h"
#include <stdint.h>

// Starting value for White
#define DEFAULT_WHITE 0xf0

// Starting value for black
#define DEFAULT_BLACK 0x10

// These mark which Pins on PORTD the different camera lines are.
// V_SYNC pulses high when a new frame is avilable,
// H_REF is high when a row is being made available on the data bus
// P_CLK pulses high for each pixel when it is made available
#define V_SYNC (1<<PD6)
#define H_REF (1<<PD3)
#define P_CLK (1<<PD2)

// The following macros are used for timing control related to waiting for changes in the control signals coming from the camera
#define waitPulseHigh( PIN ) while( !(PIND & PIN) ); while((PIND&PIN));
#define waitRisingEdge( PIN ) while( !(PIND & PIN));
#define waitFallingEdge( PIN ) while( PIND & PIN );

// Stores the current value for white
uint8_t white;

// Stores the current value for black
uint8_t black;

// Stores the threshhold that is used to determine if a pixel is white or black
uint8_t thresh;

// Used to store the current center of the blob being tracked
uint8_t objectCenterX;

// Used to store the current center of the blob being tracked
uint8_t objectCenterY;

/**
 * This function initializes the camera system so that images can be pulled from it to allow tracking
 * This functon sets PORTA as input as well as Pins 6, 3 and 2 on PORTD
 * This function also calls for the TWI subsystem to initialize
 */
void cameraInit(void) {

    // Initialize the Two Wire Interface
    TWIinit();

    // Slow down the camera pixel rate
    TWIwrite(0x11, 0x17);

    // Set output to 176x144
    TWIwrite(0x14, 0x20);

    // Set P_CLK to only pulse when HREF is high
    TWIwrite(0x39, 0x40);

    // Set the initial Threshold
    thresh = (DEFAULT_WHITE + DEFAULT_BLACK) / 2;

    // Set the PINS needed by the Camera system to be inputs
    DDRD = DDRD & ~(V_SYNC | H_REF | P_CLK);

    // Set PORTA as input so the pixel value can be retrieved.
    DDRA = 0x00;

    // Set the default white value
    white = DEFAULT_WHITE;

    // Set the default black value
    black = DEFAULT_BLACK;

    // Set the initial center values
    objectCenterX = 0x90;
    objectCenterY = 0x90;

}

/**
 * This function calibrates one of the colors used by the camera to determine
 * if a pixel is white or black. The single parameter is used to determine
 * which color is being set. This function works by looking for the highest
 * value and the lowest value and averaging them. This means that a solid white
 * or black image needs to be presented to calibrate.
 * @param calWhite  If calibrating for white this is true. If false black will
 *                  be calibrated instead.
 */
void calibrate(bool calWhite) {

    uint8_t high_val = 0;
    uint8_t low_val = 255;

    uint8_t val;

    // wait for the start of the next frame
    waitPulseHigh( V_SYNC);

    // iterate through each row
    for (int a = 1; a <= 176; a++) {

        // wait for the start of the next row
        waitRisingEdge( H_REF);

        // iterate through the columns
        for (int b = 1; b <= 144; b++) {

            // wait for the next pixel to be ready
            waitRisingEdge( P_CLK);

            // read the pixel data
            val = PINA;

            // record if it is the highest seen value or the lowest seen value
            if (val > high_val) {

                high_val = val;

            } else if (val < low_val) {

                low_val = val;

            }

        }

    }

    // if calibrating white save the average as white, if calibrating as black
    // save the average as black
    if (calWhite) {

        white = (high_val + low_val) / 2;

    } else {

        black = (high_val + low_val) / 2;

    }

    // calculaet the new threshhold
    thresh = (white + black) / 2;

}

/**
 * Getter for the white color
 * @return The current color for white
 */
uint8_t getWhite(void) {

    return white;

}

/**
 * Getter for the black color
 * @return The current color for black
 */
uint8_t getBlack(void) {

    return black;

}

/**
 * Getter for the current threshold
 * @return The current threshold
 */
uint8_t getThresh(void) {

    return thresh;

}

/**
 * Processes the next frame for tracking. Determines the location of the object
 * by looking at the first line which a white pixel appears for the x location
 * recording the first and last y value a white pixel appears on. This method
 * sets objectXCenter and objectYCenter rather than having a return
 */
void processImage(void) {

    // used to store the pixel value
    uint8_t val;

    // used to record when white pixels are seen and where
    uint8_t firstX = 255;
    uint8_t firstY = 255;
    uint8_t lastX = 0;
    uint8_t lastY = 0;

    // when true the first white pixel has been seen
    bool seenFirst = false;

    // when true the first line with white pixels is currently being looked at
    bool firstLine = true;

    // wait for the next frame
    waitPulseHigh( V_SYNC);

    // iterate through each row
    for (uint8_t y = 1; y <= 144; y++) {

        // wait for the next row
        waitRisingEdge( H_REF);

        // iterate through each pixel in the row
        for (uint8_t x = 1; x <= 176; x++) {

            // if H_REF has gone low than the row is over and break
            // out of the loop
            if (!(PIND & (H_REF))) {

                break;

            }

            // Wait for the next pixel to be ready
            waitRisingEdge( P_CLK);
            waitFallingEdge( P_CLK);

            // get the pixel value
            val = PINA;

            // if the pixel is white record its location
            if (val > thresh) {

                // if a white pixel has not been seen yet record this as the first
                if (!seenFirst) {

                    firstX = x;
                    firstY = y;

                    seenFirst = true;
                }

                // if we are looking at the first line still record the last seen
                // white pixel's x location
                if (firstLine) {

                    lastX = x;

                }

                // record this pixel as the last seen in the y location
                lastY = y;

            }

        }

        // if the first white pixel has been seen so has the first line with
        // white pixels
        if (seenFirst && firstLine) {

            firstLine = false;

        }

    }

    // record the objects center
    objectCenterX = (lastX + firstX) / 2;
    objectCenterY = (lastY + firstY) / 2;

}

/**
 * Getter for the objects center's x-location
 * @return The X location of the object's center
 */
uint8_t getObjectXCenter(void) {

    return objectCenterX;

}

/**
 * Getter for the object's center's y-location
 * @return The Y location of the object's center
 */
uint8_t getObjectYCenter(void) {

    return objectCenterY;

}
